/*
  rcmotor.c
  Controls the Motor Drive function

  Created by:  Andrew Stiegmann
  
 */

#include "rcmotor.h"
#include <pwmout.h>
#include <error.h>

#define MAX_FORWARD 150
#define MIN_FORWARD 111
#define DELTA_FORWARD (MAX_FORWARD - MIN_FORWARD)

#define MAX_REVERSE 0 
#define MIN_REVERSE 0
#define DELTA_REVERSE (MAX_REVERSE - MIN_REVERSE)

#define RCMOTOR_STOP  111

#define RCMOTOR_PWM_CHANNEL 0

#define RCMOTOR_COUNT 1

#define RC_DIVAMUX 4
#define RC_DIVAVAL 44

int RCMotor_Init( void );
int RCMotor_DeInit( void );
inline int RCMotor_SetPWM( int val );

struct _RCMotor
{
  int users;
  int speed;
} RCMotor;

int RCMotor_SetActive(int state)
{
  if ( state )
    return RCMotor_Init( );
  else
    return RCMotor_DeInit( );
}

int RCMotor_GetActive( void )
{
  return RCMotor.users;
}

int RCMotor_SetSpeed(int val)
{
  // Sets the RCMotor Speed based on a percentage
  int status;

  if ( val < -100 || val > 100 )
    return CONTROLLER_ERROR_ILLEGAL_PARAMETER_VALUE;
  
  if ( !RCMotor.users )
    {
      status = RCMotor_Init();
      if ( status != CONTROLLER_OK )
	return status;
    }

  // Now attempt to set the motor's values
  if ( val > 0 )
    {
      // We set a forward speed
      status = RCMotor_SetPWM( (val * DELTA_FORWARD / 100) + MIN_FORWARD );
    }
  else if ( val < 0 )
    {
      // We set a reverse speed
      status = RCMotor_SetPWM( (val * DELTA_REVERSE / 100) + MIN_REVERSE );
    }
  else
    {
      status = RCMotor_SetPWM(RCMOTOR_STOP);
    }

  if( status != CONTROLLER_OK )
    return status;
  
  RCMotor.speed = val;
  return CONTROLLER_OK;
}

int RCMotor_GetSpeed()
{
  return RCMotor.speed;
}

int RCMotor_Stop()
{
  if ( RCMotor.users )
    return RCMotor_SetPWM(RCMOTOR_STOP);
  else
    return CONTROLLER_ERROR_CANT_LOCK;
}

int RCMotor_Init()
{
  int status; 

  // Check if it already has a user
  if ( RCMotor.users )
    return CONTROLLER_ERROR_CANT_LOCK;
  
  // Start that channel
  status = PwmOut_SetActive( RCMOTOR_PWM_CHANNEL, 1 );

  if ( status < 0 )
    return status;

  PwmOut_SetDividerAValue( RC_DIVAVAL );
  PwmOut_SetDividerAMux( RC_DIVAMUX );
  RCMotor.users++;
  RCMotor.speed = 0;
  return RCMotor_SetPWM(RCMOTOR_STOP);
  
}

int RCMotor_DeInit()
{ 
  // Check if it already has a user
  if ( !RCMotor.users )
    return CONTROLLER_ERROR_TOO_MANY_STOPS;
  
  // Stop that channel
  RCMotor.users--;
  RCMotor.speed = 0;
  return PwmOut_SetActive( RCMOTOR_PWM_CHANNEL, 0 );
  
}

inline int RCMotor_SetPWM(int duty)
{
  // Sets the RCMotor Speed Directly
  return PwmOut_SetDuty( RCMOTOR_PWM_CHANNEL, duty );
}
  

#ifdef OSC

/** \defgroup PWMOutOSC PWM Out - OSC
  Control the RCMotor via OSC.

*/

#include "osc.h"
#include "string.h"
#include "stdio.h"

// Need a list of property names
// MUST end in zero
static char* RCMotorOsc_Name = "rcmotor";
static char* RCMotorOsc_PropertyNames[] = { "active", "speed", "stop", 
					       0 }; // must have a trailing 0

int RCMotorOsc_PropertySet( int index, int property, int value );
int RCMotorOsc_PropertyGet( int index, int property );

// Returns the name of the subsystem
const char* RCMotorOsc_GetName( )
{
  return RCMotorOsc_Name;
}

// Now getting a message.  This is actually a part message, with the first
// part (the subsystem) already parsed off.
int RCMotorOsc_ReceiveMessage( int channel, char* message, int length )
{
  int status = Osc_IndexIntReceiverHelper( channel, message, length, 
                                     RCMOTOR_COUNT, RCMotorOsc_Name,
                                     RCMotorOsc_PropertySet, RCMotorOsc_PropertyGet, 
                                     RCMotorOsc_PropertyNames );
                                     
  if ( status != CONTROLLER_OK )
    return Osc_SendError( channel, RCMotorOsc_Name, status );
  return CONTROLLER_OK;
}

// Set the index LED, property with the value
int RCMotorOsc_PropertySet( int index, int property, int value )
{
  index = 0;
  switch ( property )
  {
    case 0: 
      RCMotor_SetActive( value );
      break;      
    case 1:
      RCMotor_SetSpeed( value );
      break;
    case 2:
      RCMotor_Stop( );
      break;
  }
  return CONTROLLER_OK;
}

// Get the index LED, property
int RCMotorOsc_PropertyGet( int index, int property )
{
  index = 0;
  int value = 0;
  switch ( property )
  {
    case 0:
      value = RCMotor_GetActive( );
      break;
    case 1:
      value = RCMotor_GetSpeed( );
      break;
    case 2:
      value = RCMotor_Stop( );
      break;
  }
  return value;
}


#endif
